#include <ros/ros.h>
#include <iostream>
#include <string>
#include "mcu_correspond/mcu_data_subscriber.h"
#include "mcu_correspond/mcu_data_sender.h"
#include <locale>

class McuCommunicationTester {
private:
    ros::NodeHandle nh_;
    std::unique_ptr<McuDataSubscriber> subscriber_;
    std::unique_ptr<McuDataSender> sender_;
    
public:
    McuCommunicationTester() : nh_() {
        subscriber_ = std::make_unique<McuDataSubscriber>(nh_);
        sender_ = std::make_unique<McuDataSender>(nh_);
        
        ROS_INFO("MCU通信测试器初始化完成");
    }
    
    void testSendColorCorrectionSignal() {
        ROS_INFO("正在发送色彩校正信号 (coco = 1.0)...");
        if (subscriber_->publishDoubleData("coco", 1.0)) {
            ROS_INFO("信号发送成功");
        } else {
            ROS_ERROR("信号发送失败");
        }
    }
    
    void testSendLcdMessage(const std::string& message) {
        ROS_INFO("正在发送LCD消息: %s", message.c_str());
        if (sender_->sendJsonKeyValue("LCD", message)) {
            ROS_INFO("LCD消息发送成功");
        } else {
            ROS_ERROR("LCD消息发送失败");
        }
    }
    
    void testSendCocoReset() {
        ROS_INFO("正在重置coco信号 (coco = 0.0)...");
        if (subscriber_->publishDoubleData("coco", 0.0)) {
            ROS_INFO("coco重置发送成功");
        } else {
            ROS_ERROR("coco重置发送失败");
        }
    }
    
    void testReadCocoValue() {
        double coco_value = subscriber_->getDoubleValue("coco");
        ROS_INFO("当前coco值: %.1f", coco_value);
        
        if (subscriber_->hasData("coco")) {
            ros::Time last_update = subscriber_->getLastUpdateTime("coco");
            ROS_INFO("最后更新时间: %f", last_update.toSec());
        } else {
            ROS_WARN("没有coco数据可用");
        }
    }
    
    void monitorCocoSignal() {
        ROS_INFO("正在监控coco信号... 按Ctrl+C停止");
        
        subscriber_->subscribeDouble("coco", [this](double value) {
            ROS_INFO("接收到coco信号: %.1f", value);
            if (value == 1.0) {
                ROS_INFO("色彩校正被触发！");
                // Simulate processing
                ros::Duration(2.0).sleep();
                
                // Send completion message
                sender_->sendJsonKeyValue("LCD", "correction ok");
                sender_->sendJsonKeyValue("coco", 0.0);
                ROS_INFO("色彩校正完成，信号已发送");
            }
        });
        
        ros::spin();
    }
    
    void printMenu() {
        std::cout << "\n=== MCU通信测试菜单 ===" << std::endl;
        std::cout << "1. 发送颜色校正信号 (coco = 1.0)" << std::endl;
        std::cout << "2. 发送LCD消息" << std::endl;
        std::cout << "3. 重置coco信号 (coco = 0.0)" << std::endl;
        std::cout << "4. 读取当前coco值" << std::endl;
        std::cout << "5. 监控coco信号 (模拟颜色校正流程)" << std::endl;
        std::cout << "6. 退出" << std::endl;
        std::cout << "请选择 (1-6): ";
    }
    
    void runInteractiveTest() {
        int choice;
        std::string message;
        
        while (ros::ok()) {
            printMenu();
            std::cin >> choice;
            
            switch (choice) {
                case 1:
                    testSendColorCorrectionSignal();
                    break;
                    
                case 2:
                    std::cout << "请输入LCD消息: ";
                    std::cin.ignore();
                    std::getline(std::cin, message);
                    testSendLcdMessage(message);
                    break;
                    
                case 3:
                    testSendCocoReset();
                    break;
                    
                case 4:
                    testReadCocoValue();
                    break;
                    
                case 5:
                    monitorCocoSignal();
                    break;
                    
                case 6:
                    ROS_INFO("正在退出...");
                    return;
                    
                default:
                    std::cout << "无效选择，请重试" << std::endl;
                    break;
            }
            
            ros::Duration(0.5).sleep();
            ros::spinOnce();
        }
    }
};

void printUsage(const std::string& program_name) {
    std::cout << "MCU通信测试工具" << std::endl;
    std::cout << "用法:" << std::endl;
    std::cout << "  " << program_name << " [选项]" << std::endl;
    std::cout << std::endl;
    std::cout << "选项:" << std::endl;
    std::cout << "  --trigger      发送颜色校正触发信号" << std::endl;
    std::cout << "  --reset        重置coco信号" << std::endl;
    std::cout << "  --status       读取当前状态" << std::endl;
    std::cout << "  --monitor      监控信号" << std::endl;
    std::cout << "  --interactive  交互式测试模式 (默认)" << std::endl;
    std::cout << "  --lcd <msg>    发送LCD消息" << std::endl;
}

int main(int argc, char** argv) {
    std::setlocale(LC_ALL, "zh_CN.UTF-8");

    ros::init(argc, argv, "mcu_communication_tester");
    
    try {
        McuCommunicationTester tester;
        
        if (argc == 1) {
            // Default: interactive mode
            tester.runInteractiveTest();
        } else {
            std::string option = argv[1];
            
            if (option == "--trigger") {
                tester.testSendColorCorrectionSignal();
            } else if (option == "--reset") {
                tester.testSendCocoReset();
            } else if (option == "--status") {
                tester.testReadCocoValue();
            } else if (option == "--monitor") {
                tester.monitorCocoSignal();
            } else if (option == "--interactive") {
                tester.runInteractiveTest();
            } else if (option == "--lcd" && argc > 2) {
                std::string message = argv[2];
                tester.testSendLcdMessage(message);
            } else {
                printUsage(argv[0]);
                return 1;
            }
        }
        
    } catch (const std::exception& e) {
        ROS_ERROR("MCU通信测试器错误: %s", e.what());
        return 1;
    }
    
    return 0;
}
